/** \file    Transform.cpp
  * \brief   Buildes quadratic (general case) approximation of segments motion.
  * \date    2001 - 2003
  * \author  Fedor Chelnokov
  * \version $Header: /home/cscom1r4/cvsroot/cs/segm/src/Transform.cpp,v 1.1 2004/05/20 09:11:46 cscom1r4 Exp $
  */

#include "segm/Segmentation.h"
#include "gslmath/motionfit.h"

#include <memory>

#include "imglib/type.h"
#include "typelib/affine.h"

using std::clog;
using std::endl;

template <class T, class M>
class TransformSolver {
public:
	TransformSolver(T & t) : _result(t) {}
	static const char * name() {
		return M::name();
	}
	void prepare() {
		_mfit->prepare();
	}
	void setXDispl(unsigned i, int x) {
		_mfit->sety(i, x);
	}
	void solveX() {
		_chisq_x = _mfit->run();
		_mfit->get_transform_x(0, M(), _result, &_err);
	}
	void setYDispl(unsigned i, int y) {
		_mfit->sety(i, y);
	}
	void solveY() {
		_chisq_y = _mfit->run();
		_mfit->get_transform_y(0, M(), _result, &_err);
	}
	void report() {
		clog << "residuals x=" << sqrt(_chisq_x/_mfit->n()) 
			 << ", y=" << sqrt(_chisq_y/_mfit->n()) << "\n";
		clog << transform_err<T>(_result, _err);
	}

protected:
	double _chisq_x, _chisq_y;
	T & _result;
	T _err;
	std::auto_ptr<motionfit> _mfit;
};

template <class T>
class TranslationSolver : public TransformSolver<T, translation_model> {
public:
	TranslationSolver(T & t) : TransformSolver<T, translation_model>(t) {}
	void alloc(unsigned n) {
		_mfit.reset(new motionfit(1, n));
	}
	void segmCenter(unsigned i, const double_point &) {
		_mfit->setX(i, 0, 1.0);
	}
};

template <class T>
class RigidZoomSolver : public TransformSolver<T, rigid_zoom_model> {
public:
	RigidZoomSolver(T & t) : TransformSolver<T, rigid_zoom_model>(t) {}
	void alloc(unsigned n) {
		_mfit.reset(new motionfit(4, 2*n));
	}
	void setXDispl(unsigned i, int x) {
		_mfit->sety(2*i, x);
	}
	void solveX() {
		//nothing
	}
	void setYDispl(unsigned i, int y) {
		_mfit->sety(2*i + 1, y);
	}
	void solveY() {
		_chisq_x = _chisq_y = _mfit->run();
		_mfit->get_transform(0, rigid_zoom_model(), _result, &_err);
	}
	void segmCenter(unsigned i, const double_point & p) {
		_mfit->setX(2*i, 0, p.x);
		_mfit->setX(2*i, 1, p.y);
		_mfit->setX(2*i, 2, 1.0);
		_mfit->setX(2*i, 3, 0.0);

		_mfit->setX(2*i + 1, 0, p.y);
		_mfit->setX(2*i + 1, 1,-p.x);
		_mfit->setX(2*i + 1, 2, 0.0);
		_mfit->setX(2*i + 1, 3, 1.0);
	}
};

template <class T>
class RigidSolver : public RigidZoomSolver<T> {
public:
	RigidSolver(T & t) : RigidZoomSolver<T>(t) {}
	static const char * name() {
		return rigid_model::name();
	}
	void solveY() {
		RigidZoomSolver<T>::solveY();
		_result.rigid();
	}
};

template <class T>
class AffineSolver : public TransformSolver<T, affine_model> {
public:
	AffineSolver(T & t) : TransformSolver<T, affine_model>(t) {}
	void alloc(unsigned n) {
		_mfit.reset(new motionfit(3, n));
	}
	void segmCenter(unsigned i, const double_point & p) {
		_mfit->setX(i, 0, p.x);
		_mfit->setX(i, 1, p.y);
		_mfit->setX(i, 2, 1.0);
	}
};

template <class T>
class QuadraticSolver : public TransformSolver<T, quadratic_model> {
public:
	QuadraticSolver(T & t) : TransformSolver<T, quadratic_model>(t) {}
	void alloc(unsigned n) {
		_mfit.reset(new motionfit(6, n));
	}
	void segmCenter(unsigned i, const double_point & p) {
		_mfit->setX(i, 0, p.x);
		_mfit->setX(i, 1, p.y);
		_mfit->setX(i, 2, 1.0);
		_mfit->setX(i, 3, p.x*p.x);
		_mfit->setX(i, 4, p.x*p.y);
		_mfit->setX(i, 5, p.y*p.y);
	}
};

template <class S>
void restoreTransform(S & s, const Segmentation & segm)
{
	unsigned n = segm.count();
	clog << s.name() << " approximation\n";
	s.alloc(n);

	unsigned i;
	for (i = 0; i < n; i++)
		s.segmCenter(i, segm.segmentByIndex(i).centre);
	s.prepare();

	//x
	for (i = 0; i < n; i++)
		s.setXDispl(i, segm.segmentByIndex(i).adisplacement().x);
	s.solveX();

	//y
	for (i = 0; i < n; i++)
		s.setYDispl(i, segm.segmentByIndex(i).adisplacement().y);
	s.solveY();

	s.report();
}

template <>
void restoreTransform(QuadraticSolver<affine> &, const Segmentation &)
{
	assert(false);
}

template <class T>
void Segmentation::determineTransform(T & t, TransformClass transformClass) const
{
	switch (transformClass) {
	case TRANSFORM_AFFINE: {
		AffineSolver<T> s(t);
		restoreTransform(s, *this);
		break;
	}
	case TRANSFORM_QUADRATIC: {
		QuadraticSolver<T> s(t);
		restoreTransform(s, *this);
		break;
	}
	case TRANSFORM_TRANSLATION: {
		TranslationSolver<T> s(t);
		restoreTransform(s, *this);
		break;
	}
	case TRANSFORM_RIGID: {
		RigidSolver<T> s(t);
		restoreTransform(s, *this);
		break;
	}
	case TRANSFORM_RIGID_ZOOM: {
		RigidZoomSolver<T> s(t);
		restoreTransform(s, *this);
		break;
	}
	default:
		assert(false);
	}
}

///switch to faster version with native support for rigid model
template <>
void Segmentation::determineTransform(affine & t, TransformClass transformClass) const
{
	determineTransform2(t, transformClass);
}

template void Segmentation::determineTransform(img::QuadraticForm & t, TransformClass) const;
//template void Segmentation::determineTransform(affine & t, TransformClass) const;

